#include "customlib.c"

int speed_test(){
 /*
	int i =0;
	
 while(i<500){
   create_drive_direct(i,i);
   i = i+10 ;
 }
 
 sleep(2.0);
 
 while(i>=0){
   i = i-10 ;
   create_drive_direct(i,i);
 }
	*/
	create_drive_direct(500, 500);
	sleep(5);
 create_stop();
 }


int drive_circle(int speed, float radius, float iterations){
 
  float distance = (iterations*2*pi*radius);
  gc_distance = 0;
  create_drive(speed, radius);
  create_sensor_update();

  while(gc_distance<distance
        && gc_lbump == 0
        && gc_rbump == 0)
  {
    sleep(0.05);
    create_sensor_update();
  }
  
}


int check_for_oil(){
 create_sensor_update();
 /*if (gc_lcliff) return(-1);
 if (gc_lfcliff) return(-1);
 if (gc_rfcliff) return(1);
 if (gc_rcliff) return(1);*/
 if (gc_lbump) return(-1);
 if (gc_rbump) return(1);
 return 0;
}




int search_for_oil(){
  
 int oil = 0;
 printf("Searching for oil...");
 
 while(oil=0){
    
  printf("."); 
  float seconds = ((float)randint(6))/2;
  int choice = randint(3);
 
  switch(choice){
	  case 0:
		  create_drive_straight(300);
    	  break;
	  case 1:
		  create_drive_straight(-300);
	      break;
	  case 2:
		  create_spin_CW(randint(90));
	      break;
	  case 3:
		  create_spin_CW(randint(90));
	      break;
  }
		  
  while(seconds>0){
    if(check_for_oil()!=0){
      oil = 1;
      break;
    }
	seconds = seconds - .1;
    sleep(.08);
  }
 }
 
 
 printf("Found oilspill!\n");
 
 sleep(5.0);
 
 /*create_drive_straight(300);
 sleep(.5);
 create_spin_CW(300);
 sleep(.5);
 create_spin_CCW(300);
 sleep(1);
 create_spin_CW(300);
 sleep(.5);
 create_drive_straight(300);
 sleep(.5);*/
 
 
}




int main(){
    
 create_connect();
 create_full();
 create_sensor_update();
 
 
//Function to run goes here 
    run_for(118.0, search_for_oil);
	create_stop();

 
 create_disconnect();

}
